/*
 * Local Map Generation
 * Author: Fan Zhang
 *
 * Subscribe to : /scan, /IR_Sensor
 * Keep updating the local map when either message came
 * send out a local for path planning
 */

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <nav_msgs/GridCells.h>
#include <nav_msgs/Odometry.h>
#include <mother_ship/DirectionControl.h>
#include <mother_ship/IRArray.h>
#include <string>
#include <vector>
using namespace std;

#define PI 3.1415926
#define min(x, y) ((x) < (y) ? (x) : (y))
#define max(x, y) ((x) > (y) ? (x) : (y))

typedef struct coordinate {
	int x, y;
} coord;

// Mode control
bool use_ir, use_update_direction, visualize_all;

int map_width, map_height;
double map_res, laser_range, inflation_radius; // map resolution in meter

ros::NodeHandle *n;
ros::Publisher local_map_pub, ir_obs_map_pub; // sending local map
ros::Publisher local_path_pub, direct_control_pub; // sending local path
ros::Subscriber scan_sub;
ros::Subscriber ir_sub;
ros::Subscriber encoder_sub;
ros::Subscriber globalDirect_sub;
tf::TransformListener *trans_listener;
//costmap_2d::Costmap2DROS *costmap;

string scan_frame_name, scan_topic_name;
string encoder_topic_name;
string map_topic_name, map_frame_name;
string direct_control_topic, path_topic_name;

vector<vector<bool> > local_map;
vector<vector<bool> > inflation_map;
std::vector<coord> inf_seq_last;
std::vector<coord> ir_obs_seq;
nav_msgs::GridCells obs_grid;
nav_msgs::GridCells obs_grid_ir;
nav_msgs::GridCells obs_gird_ir_only;

// Path Plan
double cur_direct, des_direct, global_direct;
double turn_angle_bound;
double safe_dist;
double angle_score, extension_score, angle_initial_score;
double robot_width;
int robot_width_idx;

// IR sensor
vector<vector<double> > ir_queue;
vector<vector<int> > ir_queue_idx;
vector<double> ir_queue_sum;
int ir_queue_pos, ir_queue_len;

vector<double> ir_array_x;
vector<double> ir_array_y;
int ir_array_size, ir_update_count;
double ir_interval, left_ir_offset;
bool updating_scan;
int scan_id;

// Dist tracking
vector<double> depth_dist_queue, ir_dist_queue;
int dist_queue_len;

void getParameters();
void initMap();
void clearMap();
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
void irCallback(const mother_ship::IRArray::ConstPtr &msg);
void encoderCallback(const nav_msgs::Odometry::ConstPtr &msg);
void globalDirectCallback(const std_msgs::Float32::ConstPtr &msg);
double getScore(double dist, double now_angle) {
	double norm_dist_score = dist / laser_range * extension_score;
	double norm_angle_score = std::abs(now_angle - des_direct)
			/ turn_angle_bound * angle_score;
	double norm_angle_initial = std::abs(now_angle - PI / 2) / turn_angle_bound
			* angle_initial_score;
	//ROS_INFO("dist %.3lf angle2des %.3lf angle2PI/2 %.3lf dist_score %.3lf angle_score %.3lf initia_score %.3lf", d);
	return norm_dist_score - norm_angle_score + 1 - norm_angle_initial + 1;
}
void searchDirection();
double dist(coord p1, coord p2) {
	//return std::sqrt(((double) p1.x - p2.x) * ((double) p1.x - p2.x)
	//		+ ((double) p1.y - p2.y) * ((double) p1.y - p2.y));
	return std::abs(p1.y - p2.y);
}
double convertIRData(float data) {
	// 40 - safe_dist
	// 90 - 1.0
	// 180 - 2.0
	// 200 - 3.0
	data = data * (-0.5941) + 410.8275;
	if (data < 40)
		return 0.1;
	if (data < 70)
		return 0.5;
	if (data < 100)
		return 1.0;
	if (data < 130)
		return 1.15;
	if (data < 160)
		return 1.4;
	if (data < 180)
		return 1.64;
	if (data < 200)
		return 2.10;
	if (data < 220)
		return 2.70;
	if (data < 240)
		return 3.55;
	//	if (data < 260)
	//		return 4.0;
	return 4.0;
}

int main(int argc, char** argv) {

	ros::init(argc, argv, "local_map");

	if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
			ros::console::levels::Debug)) {
		ros::console::notifyLoggerLevelsChanged();
	}
	ROS_INFO("Initialize ROS Node");
	n = new ros::NodeHandle;
	trans_listener = new tf::TransformListener(ros::Duration(10));

	//	n->setParam("/local_map/costmap/global_frame", std::string("base123"));
	//	ros::NodeHandle private_nh("~/" + std::string("costmap"));
	//	string global_frame;
	//	private_nh.param("global_frame", global_frame, std::string("/map"));
	//	ROS_INFO(global_frame.c_str());

	// Initialization
	ROS_INFO("Setting Parameters");
	getParameters();
	initMap();

	//	ROS_INFO("Start Costmap");
	//	costmap = new costmap_2d::Costmap2DROS("local_cost_map", *trans_listener);
	//	costmap->start();

	// ROS communication setup
	ROS_INFO("Set up ROS communication");
	scan_sub = n->subscribe<sensor_msgs::LaserScan> (scan_topic_name, 1,
			scanCallback);
	ir_sub = n->subscribe<mother_ship::IRArray> ("IRSensor", 1, irCallback);
	encoder_sub = n->subscribe<nav_msgs::Odometry> (encoder_topic_name, 1,
			encoderCallback);
	globalDirect_sub = n->subscribe<std_msgs::Float32> ("GlobalDirection", 1,
			globalDirectCallback);
	local_map_pub = n->advertise<nav_msgs::GridCells> (map_topic_name, 1);
	ir_obs_map_pub = n->advertise<nav_msgs::GridCells> ("IRSensor_Map", 1);
	local_path_pub = n->advertise<nav_msgs::GridCells> (path_topic_name, 1);
	direct_control_pub = n->advertise<mother_ship::DirectionControl> (
			direct_control_topic, 1);

	ROS_INFO("Waiting for message");
	ros::spin();

	return 0;
}

void getParameters() {
	n->param("laser_scan_frame", scan_frame_name, std::string("laser"));
	n->param("laser_scan_topic", scan_topic_name, std::string("scan"));
	n->param("map_topic_name", map_topic_name, std::string("local_map"));
	n->param("map_frame_name", map_frame_name, std::string("laser"));
	n->param("encoder_topic", encoder_topic_name, std::string("odom"));
	n->param("local_path_topic_name", path_topic_name, std::string("laser"));
	n->param("direction_control_topic", direct_control_topic, std::string(
			"direction_control"));

	n->param("map_width", map_width, 100);
	n->param("map_height", map_height, 100);
	n->param("map_resolution", map_res, 0.05);
	n->param("laser_range", laser_range, 4.0);
	n->param("inflation_radius", inflation_radius, 0.5);
	n->param("robot_width", robot_width, 0.3);
	robot_width_idx = (int) (robot_width / map_res);

	n->param("use_ir", use_ir, false);
	n->param("use_update_direction", use_update_direction, false);
	n->param("visualization", visualize_all, true);

	n->param("safe_distance", safe_dist, 2.5);
	n->param("turn_angle_bound", turn_angle_bound, 20.0 / 180.0 * PI);
	n->param("angle_score", angle_score, 1.0);
	n->param("extension_score", extension_score, 1.0);
	n->param("angle_initial_score", angle_initial_score, 0.5);

	n->param("ir_number", ir_array_size, 7);
	n->param("front_ir_interval", ir_interval, 0.05);
	n->param("front_ir_left_offset", left_ir_offset, -0.1);
	n->param("ir_queue_length", ir_queue_len, 10);

	n->param("distance_track_queue_length", dist_queue_len, 10);
	//	n->setParam("/local_map/local_cost_map/robot_base_frame", scan_frame_name);
	//	n->setParam("/local_map/local_cost_map/global_frame", map_frame_name);
	//	n->setParam("/local_map/local_cost_map/observation_sources", "laser_scan");
	//	n->setParam("/local_map/local_cost_map/laser_scan/topic", "scan");
	//	n->setParam("/local_map/local_cost_map/laser_scan/sensor_frame", "laser");
	//	n->setParam("/local_map/local_cost_map/laser_scan/data_type", "LaserScan");
}
void initMap() {
	local_map.resize(map_height);
	for (int i = 0; i < map_height; i++)
		local_map[i].resize(map_width);
	inflation_map.resize(map_height);
	for (int i = 0; i < map_height; i++)
		local_map[i].resize(map_width);
	for (int i = 0; i < map_height; i++)
		for (int j = 0; j < map_width; j++)
			inflation_map[i][j] = false;
	cur_direct = PI / 2;
	global_direct = PI / 2;
	des_direct = PI / 2;

	ir_update_count = 0;
	ir_array_x.resize(ir_array_size);
	ir_array_y.resize(ir_array_size);
	for (int i = 0; i < ir_array_size; i++) {
		ir_array_x[i] = left_ir_offset + ir_interval * i;
	}
	updating_scan = false;

	ir_queue.resize(ir_array_size);
	ir_queue_idx.resize(ir_array_size);
	ir_queue_sum.resize(ir_array_size);
	for (int i = 0; i < ir_array_size; i++)
		ir_queue_sum[i] = 0.0;
	ir_queue_pos = -1;

	depth_dist_queue.resize(10);
	ir_dist_queue.resize(10);
}
void clearMap() {
	if (inf_seq_last.size() > 0) {
		for (int i = 0; i < inf_seq_last.size(); i++) {
			inflation_map[inf_seq_last[i].y][inf_seq_last[i].x] = false;
		}
	}
	inf_seq_last.clear();
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
	updating_scan = true;
	scan_id = (scan_id + 1) % 1000;
	// renew map
	clearMap();
	// Create obstacle grid
	if (visualize_all) {
		obs_grid.header.frame_id = map_frame_name;
		obs_grid.header.stamp = ros::Time();
		obs_grid.cell_width = map_res;
		obs_grid.cell_height = map_res;
		obs_grid.cells.clear();
	}

	// Robot coordinates in local frame
	double x_coord_o = 0.0;
	double y_coord_o = 0.0;
	// Map index
	/* y, map_height, PI / 2 degree
	 * ^
	 * |
	 * |
	 * |____._____> x, map_width, 0 degree
	 *      x_o
	 */
	bool too_close = true;
	int nan_num = 0;
	int x_idx_o = (int) (map_width / 2), y_idx_o = 0;
	// Convert scan data to x, y coordinates
	for (int i = 2; i < scan->ranges.size() - 1; i++) {
		// Get range data and angle of one read
		double range = i > 0 ? (scan->ranges[i] + scan->ranges[i - 1]
				+ scan->ranges[i + 1]) / 3 : scan->ranges[i];
		double angle = (double) i * scan->angle_increment + scan->angle_min;
		// Filter scan data
		if (isnan(range)) {
			nan_num++;
			//range = 0.5;
			continue;
		}
		if (std::abs(angle) > PI / 2 || range > laser_range)
			continue;
		too_close = false;
		// Convert to normal angle representation
		angle += PI / 2;
		// Get coordinate in local frame
		double x = range * cos(angle), y = range * sin(angle);
		int x_idx = (int) (x / map_res) + x_idx_o, y_idx = (int) (y / map_res)
				+ y_idx_o;
		if (x_idx < 0 || y_idx < 0 || x_idx >= map_width || y_idx >= map_height)
			continue;

		geometry_msgs::Point point;
		//		point.x = scan->ranges[i] * cos(angle - PI / 2);
		//		point.y = scan->ranges[i] * sin(angle - PI / 2);
		//		point.z = 0;
		//		obs_grid.cells.push_back(point);
		// Inflation
		coord xy;
		xy.x = x_idx;
		xy.y = y_idx;
		//		inf_seq_last.push_back(xy);
		//		local_map[y_idx][x_idx] = true;
		int max_inf = (int) (inflation_radius / map_res) + 1;
		for (int ii = max(0, x_idx - max_inf); ii
				< min(map_width, x_idx + max_inf); ii++)
			for (int jj = max(0, y_idx - max_inf); jj
					< min(map_height, y_idx + max_inf); jj++) {
				coord xy1;
				xy1.x = ii;
				xy1.y = jj;
				if (dist(xy, xy1) < inflation_radius / map_res) {
					inf_seq_last.push_back(xy1);
					inflation_map[xy1.y][xy1.x] = true;
					point.x = jj * map_res;
					point.y = (x_idx_o - ii) * map_res;
					point.z = 0;
					if (visualize_all) {
						obs_grid.cells.push_back(point);
					}
				}
			}
	}

	if (visualize_all)
		local_map_pub.publish(obs_grid);

	if (too_close || scan->ranges.size() / 3 < nan_num) {
		mother_ship::DirectionControl direct_msg;
		direct_msg.desired_angle_offset = 0;
		direct_msg.dist_front = 0.0;
		direct_control_pub.publish(direct_msg);
	} else
		searchDirection();
}

double findMedian(vector<double> &data) {
	vector<double> tmpData;
	tmpData.resize(data.size());
	for (int i = 0; i < data.size(); i++)
		tmpData[i] = data[i];
	for (int i = 0; i < data.size() - 1; i++)
		for (int j = i + 1; j < data.size(); j++)
			if (tmpData[i] < tmpData[j]) {
				double tmp = tmpData[i];
				tmpData[i] = tmpData[j];
				tmpData[j] = tmp;
			}
	return tmpData[data.size() / 2];
}
void irCallback(const mother_ship::IRArray::ConstPtr &msg) {
	if (!use_ir)
		return;
	float data[ir_array_size - 2];
	data[0] = msg->ir_1;
	data[1] = msg->ir_2;
	data[2] = msg->ir_3;
	data[3] = msg->ir_4;
	data[4] = msg->ir_5;
	// ir median filter
	ir_queue_pos = (ir_queue_pos + 1) % ir_queue_len;
	if (ir_queue[0].size() < ir_queue_len) {
		// if not enough data
		for (int i = 0; i < ir_array_size - 2; i++) {
			ir_queue[i].push_back(data[i]);
			ir_queue_sum[i] += data[i];
			//data[i] = ir_queue_sum[i] / ir_queue[i].size();
			data[i] = findMedian(ir_queue[i]);

		}
		return;
	} else {
		// update data
		for (int i = 0; i < ir_array_size - 2; i++) {
			ir_queue_sum[i] -= ir_queue[i][ir_queue_pos];
			ir_queue[i][ir_queue_pos] = data[i];
			ir_queue_sum[i] += data[i];
			//data[i] = ir_queue_sum[i] / ir_queue[i].size();
			data[i] = findMedian(ir_queue[i]);
		}
	}
	//ROS_INFO("%f %f %f %f %f", data[0], data[1], data[2], data[3], data[4]);
	data[0] = convertIRData(data[0]);
	data[1] = convertIRData(data[1]);
	data[2] = convertIRData(data[2]);
	data[3] = convertIRData(data[3]);
	data[4] = convertIRData(data[4]);
	data[0] = data[1];

	//ROS_INFO("%f %f %f %f %f %d", data[0], data[1], data[2], data[3], data[4], (int)ir_queue.size());
	// delete previous obstacles
	if (ir_obs_seq.size() > 0) {
		for (int i = 0; i < ir_obs_seq.size(); i++) {
			inflation_map[ir_obs_seq[i].y][ir_obs_seq[i].x] = false;
		}
	}
	ir_obs_seq.clear();

	if (visualize_all) {
		obs_grid_ir.cells.clear();
		std::copy(obs_grid.cells.begin(), obs_grid.cells.end(),
				obs_grid_ir.cells.begin());
		obs_grid_ir.header.frame_id = map_frame_name;
		obs_grid_ir.header.stamp = ros::Time();
		obs_grid_ir.cell_width = map_res;
		obs_grid_ir.cell_height = map_res;
		obs_gird_ir_only.header.frame_id = map_frame_name;
		obs_gird_ir_only.header.stamp = ros::Time();
		obs_gird_ir_only.cell_width = map_res;
		obs_gird_ir_only.cell_height = map_res;
	}

	bool too_close = false;
	// Add obstacles for IR sensors
	int x_idx_o = (int) (map_width / 2), y_idx_o = 0;
	for (int i = 0; i < ir_array_size - 2; i++) {
		double x = ir_array_x[i], y = data[i];

		//ROS_INFO("%lf %lf %d \n", x, y, i);

		if (y <= safe_dist) {
			too_close = true;
			break;
		}
		int x_idx = (int) (x / map_res) + x_idx_o, y_idx = (int) (y / map_res)
				+ y_idx_o;
		if (x_idx < 0 || y_idx < 0 || x_idx >= map_width || y_idx >= map_height)
			continue;

		coord xy;
		xy.x = x_idx;
		xy.y = y_idx;

		geometry_msgs::Point point;
		//ir_obs_seq.push_back(xy);
		//inflation_map[xy.x][xy.y] = true;

		int max_inf = (int) (inflation_radius / map_res) + 1;
		for (int ii = max(0, x_idx - max_inf); ii
				< min(map_width, x_idx + max_inf); ii++)
			for (int jj = max(0, y_idx - max_inf); jj
					< min(map_height, y_idx + max_inf); jj++) {
				coord xy1;
				xy1.x = ii;
				xy1.y = jj;
				if (dist(xy, xy1) < inflation_radius / map_res) {
					ir_obs_seq.push_back(xy1);
					inflation_map[xy1.y][xy1.x] = true;
					point.x = jj * map_res;
					point.y = (x_idx_o - ii) * map_res;
					point.z = 0;
					if (visualize_all) {
						obs_grid_ir.cells.push_back(point);
						obs_gird_ir_only.cells.push_back(point);
					}
				}
			}
	}

	//	msg->ir_r
	//	msg->ir_l
	//ROS_INFO("Obstacle Cell Number %d \n", ir_obs_seq.size());

	if (visualize_all) {
		local_map_pub.publish(obs_grid_ir);
		ir_obs_map_pub.publish(obs_gird_ir_only);
	}

	if (too_close) {
		mother_ship::DirectionControl direct_msg;
		direct_msg.desired_angle_offset = 0;
		direct_msg.dist_front = 0.0;
		direct_control_pub.publish(direct_msg);
	} else {
		searchDirection();
	}

}
void encoderCallback(const nav_msgs::Odometry::ConstPtr &msg) {
}
void globalDirectCallback(const std_msgs::Float32::ConstPtr &msg) {
	if (use_update_direction) {
		// forward is zero right side is smaller than zero, in degree
		des_direct = msg->data / 180.0 * PI + PI / 2;
	} else
		des_direct = PI / 2;
}

void searchDirection() {
	double angle_step = 0.05, angle_incre = 0.0;
	double best_angle = -1, best_score = 0, best_dist = 0;
	bool too_close = false;
	// angle on the right side of PI / 2
	double max_right_angle = des_direct - turn_angle_bound;
	double max_left_angle = des_direct + turn_angle_bound;

	//double max_right_angle = (cur_direct + des_direct) / 2 - turn_angle_bound;
	//double max_left_angle = (cur_direct + des_direct) / 2 + turn_angle_bound;

	// Search Ahead
	int x_idx_o = (int) (map_width / 2);
	int x_idx = x_idx_o, y_idx = 0;
	int valid_dist = 0;
	coord start_pos, final_pos;
	start_pos.x = x_idx;
	start_pos.y = y_idx;

	//while (++y_idx < map_height && !inflation_map[y_idx][x_idx]);

	while (++y_idx < map_height) {
		x_idx = x_idx_o;
		int width_idx_half = robot_width_idx / 2 + 1;
		//if (x_idx - width_idx_half >= 0 && inflation_map[y_idx][x_idx - width_idx_half])
		//	break;
		//if (x_idx + width_idx_half < map_width && inflation_map[y_idx][x_idx + width_idx_half])
		//	break;
		if (inflation_map[y_idx][x_idx] || inflation_map[y_idx][x_idx
				- width_idx_half] || inflation_map[y_idx][x_idx
				+ width_idx_half])
			break;
	}

	final_pos.x = x_idx;
	final_pos.y = y_idx;
	best_angle = PI / 2;

	best_score
			= dist(start_pos, final_pos) * map_res > safe_dist ? extension_score
					* (dist(start_pos, final_pos) * map_res / laser_range)
					- angle_score * (std::abs(best_angle - des_direct)
							/ turn_angle_bound) + 1
					: 0;
	best_dist = dist(start_pos, final_pos) * map_res;
	// Search neighbor angles
	//ROS_INFO("ra %lf la %lf", max_right_angle * 180 / PI, max_left_angle * 180 / PI);

	double now_angle = max_right_angle - angle_step;
	while (now_angle <= max_left_angle) {
		now_angle += angle_step;
		if (std::abs(now_angle - PI / 2) < 0.01)
			continue;
		x_idx = (int) (map_width / 2);
		y_idx = 0;
		// ray tracing
		while (++y_idx < map_height && y_idx > 0) {
			x_idx = x_idx_o + (int) ((double) y_idx / tan(now_angle));
			int width_idx_half = robot_width_idx / 2 + 1;
			if (x_idx - width_idx_half < 0 || x_idx + width_idx_half
					>= map_width)
				break;
			//if (x_idx - width_idx_half >= 0 && inflation_map[y_idx][x_idx - width_idx_half])
			//	break;
			//if (x_idx + width_idx_half < map_width && inflation_map[y_idx][x_idx + width_idx_half])
			//	break;
			if (inflation_map[y_idx][x_idx] || inflation_map[y_idx][x_idx
					- width_idx_half] || inflation_map[y_idx][x_idx
					+ width_idx_half])
				break;
		}
		final_pos.x = x_idx;
		final_pos.y = y_idx;
		if (dist(start_pos, final_pos) * map_res < safe_dist * cos(std::abs(PI
				/ 2 - now_angle))) {
			too_close = true;
			break;
		}

		// update best score
		double norm_dist_score = dist(start_pos, final_pos) * map_res
				/ laser_range * extension_score;
		double norm_angle_score = 1 - std::abs(now_angle - des_direct)
				/ turn_angle_bound * angle_score;
		double norm_angle_initia = 1 - std::abs(now_angle - cur_direct) / 2
				/ turn_angle_bound * angle_initial_score;

		if (norm_dist_score + norm_angle_score + norm_angle_initia
				+ norm_angle_initia > best_score) {
			best_angle = now_angle;
			best_score = norm_dist_score + norm_angle_score + norm_angle_initia
					+ norm_angle_initia;
			//	ROS_INFO("id %d d %.2lf d_s %.2lf a %.2lf a_s %.2lf last_a %.2lf initia_s %.2lf all_s %.2lf", scan_id, dist(start_pos, final_pos) * map_res, norm_dist_score, ((now_angle - des_direct) / PI * 180.0), norm_angle_score - 1, (cur_direct - PI / 2) / PI * 180.0, norm_angle_initia, best_score);
			best_dist = dist(start_pos, final_pos) * map_res;
		}
	}
	cur_direct = best_angle;
	if (best_dist > 5.0)
		best_dist = 5.0;
	if (best_dist < safe_dist * cos(std::abs(PI / 2 - best_angle)))
		too_close = true;
	if (best_score == 0 || too_close) {
		ROS_INFO("Best distance is %f, have to stop.", best_dist);
		mother_ship::DirectionControl direct_msg;
		direct_msg.desired_angle_offset = 0;
		direct_msg.dist_front = 0.0;
		direct_control_pub.publish(direct_msg);
		return;
	}

	// Generate Path Visualization
	nav_msgs::GridCells path_grid;
	path_grid.header.frame_id = map_frame_name;
	path_grid.header.stamp = ros::Time();
	path_grid.cell_width = map_res;
	path_grid.cell_height = map_res;
	geometry_msgs::Point point;
	x_idx = (int) (map_width / 2);
	y_idx = 0;

	//send direction control signal
	mother_ship::DirectionControl direct_msg;
	direct_msg.desired_angle_offset = (best_angle - PI / 2.0) / PI * 180.0;
	direct_msg.dist_front = best_dist;
	direct_control_pub.publish(direct_msg);
	sleep(0.01);

	if (visualize_all) {
		if (best_angle == PI / 2) {
			while (++y_idx < map_height && !inflation_map[y_idx][x_idx]) {
				int width_idx_half = robot_width_idx / 2 + 1;
				point.x = y_idx * map_res;
				point.y = (x_idx_o - x_idx) * map_res;
				point.z = 0;
				path_grid.cells.push_back(point);
			}
		} else {
			// ray tracing
			while (++y_idx < map_height && y_idx > 0) {
				x_idx = x_idx_o + (int) ((double) y_idx / tan(best_angle));
				int width_idx_half = robot_width_idx / 2 + 1;
				if (x_idx - width_idx_half < 0 || x_idx + width_idx_half
						>= map_width)
					break;
				//if (x_idx - width_idx_half >= 0 && inflation_map[y_idx][x_idx - width_idx_half])
				//	break;
				//if (x_idx + width_idx_half < map_width && inflation_map[y_idx][x_idx + width_idx_half])
				//	break;
				if (inflation_map[y_idx][x_idx] || inflation_map[y_idx][x_idx
						- width_idx_half] || inflation_map[y_idx][x_idx
						+ width_idx_half])
					break;
				point.x = y_idx * map_res;
				point.y = (x_idx_o - x_idx) * map_res;
				point.z = 0;
				path_grid.cells.push_back(point);
				point.x = y_idx * map_res;
				point.y = (x_idx_o - x_idx - width_idx_half) * map_res;
				point.z = 0;
				path_grid.cells.push_back(point);
				point.x = y_idx * map_res;
				point.y = (x_idx_o - x_idx + width_idx_half) * map_res;
				point.z = 0;
				path_grid.cells.push_back(point);
			}
		}
		local_path_pub.publish(path_grid);
	}
}
